from robot_control_interface.action import Moverobot
import math
class Robot():
    def __init__(self) -> None:
        self.current_pose = 0.0
        self.target_pose = 0.0
        self.move_distance = 0.0
        self.status = Moverobot.Feedback
        
    def get_status(self):
        return self.status
    
    def get_current_pose(self):
        return self.current_pose

    def close_goal(self):
        return math.fabs(self.target_pose - self.current_pose) < 0.01

    def stop_move(self):
        self.status = Moverobot.Feedback.STATUS_STOP

    def move_step(self):
        direct = self.move_distance / math.fabs(self.move_distance)
        step = direct * math.fabs(self.target_pose - self.current_pose) * 0.1
        self.current_pose += step  
        print(f"move：{step}, current location：{self.current_pose}")
        return self.current_pose

    def set_goal(self, distance):
        self.move_distance = distance
        self.target_pose += distance 

        if self.close_goal():
            self.stop_move()
            return False

        self.status = Moverobot.Feedback.STATUS_MOVEING 
        return True
